#include <gtest/gtest.h>
#include <behaviortree_cpp_v3/behavior_tree.h>
#include <behaviortree_cpp_v3/bt_factory.h>
#include <logger/logger.h>

BT::NodeStatus aa()
{
    EASE_INFO("aa");
    return BT::NodeStatus::SUCCESS;
}

BT::NodeStatus bb()
{
    EASE_INFO("bb");
    return BT::NodeStatus::FAILURE;
}

BT::NodeStatus cc()
{
    EASE_INFO("cc");
    return BT::NodeStatus::FAILURE;
}

BT::NodeStatus dd()
{
    EASE_INFO("dd");
    return BT::NodeStatus::FAILURE;
}

BT::NodeStatus ee()
{
    EASE_INFO("ee");
    return BT::NodeStatus::FAILURE;
}

BT::NodeStatus ff()
{
    EASE_INFO("ff");
    return BT::NodeStatus::SUCCESS;
}

TEST(behavior_tree_sequence_func, behavior_tree)
{
    const char *xml_text_sequence_func = R"(
        <root>
            <BehaviorTree ID="MainTree">
                <Sequence>
                    <aa/>
                    <bb/>
                    <cc/>
                </Sequence>
            </BehaviorTree>
        </root>
        )";

    BT::BehaviorTreeFactory factory;
    factory.registerSimpleCondition("aa", std::bind(aa));
    factory.registerSimpleCondition("bb", std::bind(bb));
    factory.registerSimpleCondition("cc", std::bind(cc));

    BT::Tree tree = factory.createTreeFromText(xml_text_sequence_func);
    tree.tickRoot();
}

TEST(behavior_tree_fallback_func, behavior_tree)
{
    const char *xml_text_fallback_func = R"(
        <root>
            <BehaviorTree ID="MainTree">
                <Fallback>
                    <aa/>
                    <bb/>
                    <cc/>
                </Fallback>
            </BehaviorTree>
        </root>
        )";

    BT::BehaviorTreeFactory factory;
    factory.registerSimpleCondition("aa", std::bind(aa));
    factory.registerSimpleCondition("bb", std::bind(bb));
    factory.registerSimpleCondition("cc", std::bind(cc));

    BT::Tree tree = factory.createTreeFromText(xml_text_fallback_func);
    tree.tickRoot();
}

TEST(behavior_tree_sequence_fallback_func, behavior_tree)
{
    const char *xml_text_fallback_func = R"(
        <root>
            <BehaviorTree ID="MainTree">
                <Sequence>
                    <Fallback>
                        <aa/>
                        <bb/>
                    </Fallback>
                    <cc/>
                    <dd/>
                    <ee/>
                </Sequence>
            </BehaviorTree>
        </root>
        )";

    BT::BehaviorTreeFactory factory;
    factory.registerSimpleCondition("aa", std::bind(aa));
    factory.registerSimpleCondition("bb", std::bind(bb));
    factory.registerSimpleCondition("cc", std::bind(cc));
    factory.registerSimpleCondition("dd", std::bind(dd));
    factory.registerSimpleCondition("ee", std::bind(ee));

    BT::Tree tree = factory.createTreeFromText(xml_text_fallback_func);
    tree.tickRoot();
}

TEST(behavior_tree_reactive_fall_back_func, behavior_tree)
{
    const char *xml_text_fallback_func = R"(
        <root>
            <BehaviorTree ID="MainTree">
                <ReactiveFallback>
                        <aa/>
                        <bb/>
                    <Sequence>
                        <cc/>
                        <dd/>
                    </Sequence>
                    <Sequence>
                        <ee/>
                        <ff/>
                    </Sequence>
                </ReactiveFallback>
            </BehaviorTree>
        </root>
        )";

    BT::BehaviorTreeFactory factory;
    factory.registerSimpleCondition("aa", std::bind(aa));
    factory.registerSimpleCondition("bb", std::bind(bb));
    factory.registerSimpleCondition("cc", std::bind(cc));
    factory.registerSimpleCondition("dd", std::bind(dd));
    factory.registerSimpleCondition("ee", std::bind(ee));
    factory.registerSimpleCondition("ff", std::bind(ff));

    BT::Tree tree = factory.createTreeFromText(xml_text_fallback_func);
    tree.tickRoot();
}

TEST(behavior_tree_if_then_else_func, behavior_tree)
{
    const char *xml_text_if_then_else_func = R"(
        <root>
            <BehaviorTree ID="MainTree">
                <IfThenElse>
                    <aa/>
                    <bb/>
                    <cc/>
                </IfThenElse>
            </BehaviorTree>
        </root>
        )";

    BT::BehaviorTreeFactory factory;
    factory.registerSimpleCondition("aa", std::bind(aa));
    factory.registerSimpleCondition("bb", std::bind(bb));
    factory.registerSimpleCondition("cc", std::bind(cc));

    BT::Tree tree = factory.createTreeFromText(xml_text_if_then_else_func);
    tree.tickRoot();
}

TEST(behavior_tree_delay_func, behavior_tree)
{
    const char *xml_text_delay_func = R"(
        <root>
            <BehaviorTree ID="MainTree">
                <Delay delay_msec="5000">
                    <aa/>
                </Delay>
            </BehaviorTree>
        </root>
        )";

    BT::BehaviorTreeFactory factory;
    factory.registerSimpleCondition("aa", std::bind(aa));
    factory.registerSimpleCondition("bb", std::bind(bb));
    factory.registerSimpleCondition("cc", std::bind(cc));

    BT::Tree tree = factory.createTreeFromText(xml_text_delay_func);
    BT::NodeStatus status = tree.tickOnce();
    while (status == BT::NodeStatus::RUNNING)
    {
        tree.sleep(std::chrono::milliseconds(500));
        status = tree.tickOnce();
    }
    EASE_INFO("done");
}

TEST(behavior_tree_repeat_func, behavior_tree)
{
    const char *xml_text_repeat_func = R"(
        <root>
            <BehaviorTree ID="MainTree">
                <Repeat num_cycles="3">
                    <aa/>
                </Repeat>
            </BehaviorTree>
        </root>
        )";

    BT::BehaviorTreeFactory factory;
    factory.registerSimpleCondition("aa", std::bind(aa));

    BT::Tree tree = factory.createTreeFromText(xml_text_repeat_func);
    BT::NodeStatus status = tree.tickOnce();
    while (status == BT::NodeStatus::RUNNING)
    {
        tree.sleep(std::chrono::milliseconds(500));
        status = tree.tickOnce();
    }
    EASE_INFO("done");
}

